/*!
 *  \file       Fft.cpp
 *  \brief      Implementazione della classe \c Fft
 *  \author     Davide Scola <S2299453@studenti.unige.it>
 *  \author		Minetti Alberto <S3064777@studenti.unige.it>
 *  \version    1.0
 *
 */

#include <vector>
#include <complex>
#include "Fft.hpp"
#include "InvalidSizeException.hpp"

using namespace std;

/*!
 *  \brief      Controlla se il numero è una potenza di 2
 *  \param[in]	number	il numero da controllare
 *  \return	true se \a number è una potenza di 2
 *
 */
template < class tType > inline bool
Fft< tType >::isPowerOfTwo( const unsigned long int number )
{
    return ( number ) && !( number & ( number - 1 ) );
}

/*!
 *  \brief      Esegue il logaritmo in base 2
 *  \param[in]	x	l'argomento del log2
 *  \return	il logaritmo in base 2 di \a x
 *
 */
template < class tType > inline unsigned long int
Fft< tType >::fastLog2(const unsigned long int x)
{
	unsigned long int y;
	
	asm ( "bsr %1, %0\n"
		: "=r"(y)
		: "r" (x)
	);
	
	return y;
}

/*!
 *  \brief      Esegue il bitreversal
 *  \param[in]	iCounter	numero di passi
 *  \param[in]	size		dimensione dell'array di output
 *  \param[out]	indArray	l'array che conterrà il bitreversal
 *
 */
template < class tType > void
Fft< tType >::bit_reversal(const unsigned long int iCounter, const unsigned long int size, std::vector< unsigned long int >& indArray)
{
    unsigned long int k = 1;
    
    indArray.resize( size, 0 );
        
	for(unsigned long int j = 0; j < iCounter; j++) {
        for(unsigned long int i = 0; i < k; i++) {
            indArray[i] <<= 1;
            indArray[i + k] = indArray[i] + 1;
        }
        
        k <<= 1;
    }
}

/*!
 *  \brief      Implementazione dell'algoritmo a farfalla
 *  \param[in]	wArray      array ausiliario W
 *
 */
template < class tType > void
Fft< tType >::doButterfly( std::vector< complex_t > wArray )
{
    Fft swap( this->size() );
	unsigned long int sComponent, iComponent, numComponents, iIndex;
    
    for(unsigned long int iCount = 0; iCount < this->p ; ++iCount)
	{
        iIndex = 0;
        
		for(unsigned long int iButterfly = 0; iButterfly < POWER_OF_2( iCount ); ++iButterfly)
		{ 
			sComponent = this->size() / POWER_OF_2( iCount ) * iButterfly;
			iComponent = ( this->size() / POWER_OF_2( iCount ) * ( iButterfly + 1 ) ) - 1;
			numComponents = this->size() / POWER_OF_2( iCount + 1 );

			for(unsigned long int iSwap = sComponent; iSwap <= sComponent + (iComponent - sComponent) / 2; ++iSwap)
			{
				swap[ iSwap ] = ( (*this)[ iSwap ] * wArray[ 0 ] ) +
                    ( (*this)[ iSwap + numComponents ] * wArray[ this->ind[ iIndex ] ] );
			}

			iIndex++;

			for(unsigned long int iSwap = sComponent + (iComponent - sComponent) / 2 + 1; iSwap <= iComponent; iSwap++)
			{
				swap[ iSwap ] = ( (*this)[ iSwap ] * wArray[ this->ind[ iIndex ] ] ) +
                    ( (*this)[ iSwap - numComponents ] * wArray[ 0 ] );
			}

			iIndex++;
		}
        
        *this = swap;
	}
    
    //bit reversal
    for(unsigned long int iCounter = 0; iCounter < this->size(); ++iCounter)
    {
        (*this)[iCounter] = swap[ this->ind[ iCounter ] ];
    }
}

/*!
 *  \brief      Calcolo effettivo della Fft
 */
template < class tType > void
Fft< tType >::doit( void )
{
  vector< complex_t > wArray( this->size() );
  
  wArray[0] = complex_t( 1, 0 );
  wArray[1] = complex_t( cos(-2 * M_PI / this->size()), sin(-2 * M_PI / this->size()) );
  
  for(unsigned long int i = 2; i < wArray.size(); i++)
    wArray[i] = wArray[i-1] * wArray[1];
  
  this->doButterfly( wArray );
}


/*!
 *  \brief      Reinizializza la FFT con il nuovo numero di punti
 *  \param[in]	numPoints	numero di punti
 *
 */
template < class tType > void
Fft< tType >::reset(const unsigned long int numPoints)
{
    this->p = this->fastLog2( numPoints );
    this->bit_reversal( this->p, numPoints, this->ind );
}

/*!
 *  \brief      Costruttore di \c Fft
 *  \param[in]	numPoints	numero di punti
 *
 * \exception	InvalidSizeException	se il numero di punti non e` una potenza di due
 *
 */
template < class tType > Fft< tType >::Fft(const unsigned long int numPoints) : vector< complex_t >( numPoints )
{
    if( !this->isPowerOfTwo( numPoints ) ) {
        throw InvalidSizeException( );
    }
    
    this->reset( numPoints );
}

/*!
 *  \brief      Calcola la Fft dato un segnale \a signal reale
 *  \param[in]	signal		segnale
 *
 * \exception	InvalidSizeException	se il segnale non ha la stessa dimensione della FFT
 */
template < class tType > void
Fft< tType >::compute(const std::vector< tType > signal)
{
  if( this->size() != signal.size() ) {
    throw InvalidSizeException( );
  }
  
  for(unsigned long int i = 0; i < signal.size(); i++)
    (*this)[i] = complex_t( signal[i], 0 );
  
  this->doit( );
}

/*!
 *  \brief      Calcola la Fft dato un segnale \a signal complesso
 *  \param[in]	signal		segnale
 *
 *  \exception	InvalidSizeException	se il segnale non ha la stessa dimensione della FFT
 */
template < class tType > void
Fft< tType >::compute(const std::vector< complex_t > signal)
{
  if( this->size() != signal.size() ) {
    throw InvalidSizeException( );
  }
  
  for(unsigned long int i = 0; i < signal.size(); i++)
    (*this)[i] = signal[i];
  
  this->doit( );
}

/*!
 *  \brief      Controlla se la Fft è reale
 *  \return		true se la Fft è reale
 *
 */
template < class tType > bool
Fft< tType >::isReal(void)
{
    for(typename Fft< tType >::iterator iter = this->begin(); iter != this->end(); ++iter)
    {
        if( iter->imag() != tType( 0 ) ) {
            return false;
        }
    }
    
    return true;
}

/*!
 *  \brief      Controlla se la Fft è pari
 *  \return		true se la Fft è pari
 *
 */
template < class tType > bool
Fft< tType >::isEven(void)
{
    unsigned long int iCount, sCount;
    
    for(iCount = 1, sCount = this->size() - 1; iCount != sCount; ++iCount, --sCount)
    {
        if( (*this)[ iCount ].real() != (*this)[ sCount ].real() ) {
            return false;
        }
    }
    
    return true;
}

/*!
 *  \brief      Calcola la Fft inversa
 */
template < class tType > void
Fft< tType >::invert( void )
{
    vector< complex_t > wArray( this->size() );
    
    wArray[0] = complex_t( 1, 0 );
    wArray[1] = complex_t( cos(-2 * M_PI / this->size()), -sin(-2 * M_PI / this->size()) );
    
    for(unsigned long int i = 2; i < wArray.size(); ++i)
        wArray[i] = wArray[i-1] * wArray[1];
    
    this->doButterfly( wArray );
    
    // normalize
    for(unsigned long int i = 0; i < this->size(); ++i)
        (*this)[i] /= this->size();
}
